/* Author: Binh Cao */

#include "CameraPlanner.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
#include "flyingcamera/base/CameraSpace.h"
#include "CameraStateValidityCheckerM.h"
#include "ompl/base/goals/GoalState.h"
#include <limits>
namespace fb = flyingcamera::base;

flyingcamera::base::moving::CameraPlanner::CameraPlanner(const ompl::base::SpaceInformationPtr &si) : ompl::base::Planner(si, "CameraPlanner")
{
    specs_.approximateSolutions = false;
    specs_.directed = true;

    timeRange_ = 0.005;
    lastGoalMotion_ = NULL;
    solutionFound_ = false;
}

flyingcamera::base::moving::CameraPlanner::~CameraPlanner(void)
{
    freeMemory();
}

void flyingcamera::base::moving::CameraPlanner::clear(void)
{
    Planner::clear();
    sampler_.reset();
    freeMemory();
    lastGoalMotion_ = NULL;
}

void flyingcamera::base::moving::CameraPlanner::setup(void)
{
    Planner::setup();
    ompl::tools::SelfConfig sc(si_, getName());
}

void flyingcamera::base::moving::CameraPlanner::freeMemory(void)
{
    for (unsigned int i = 0 ; i < tree_.size() ; ++i)
    {
    	if (tree_[i]->state)
        si_->freeState(tree_[i]->state);
        delete tree_[i];
    }
}

ompl::base::PlannerStatus flyingcamera::base::moving::CameraPlanner::solve(const ompl::base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    ompl::base::Goal	*goal   = pdef_->getGoal().get();
    const ompl::base::State *start = pdef_->getStartState(0);
    if (tree_.size() == 0)
    {
    	Motion *motion = new Motion(si_);
    	si_->copyState(motion->state, start);
    	tree_.push_back(motion);
    }

    //ompl::base::State *tmpState = si_->allocState();
    //flyingcamera::base::CameraSpace::StateType* temp;

    const ompl::base::State *sg = goal->as<ompl::base::GoalState>()->getState();

    const flyingcamera::base::CameraSpace::StateType *csg=sg->as<flyingcamera::base::CameraSpace::StateType>();

    if (!sampler_)
	sampler_ = si_->allocValidStateSampler();

    Motion *solution  = NULL;
    ompl::base::State *rstate = si_->allocState();
    ompl::base::State *xstate = si_->allocState();
    bool solved = false;

    /*sampler_->sample(tmpState);
        temp=tmpState->as<flyingcamera::base::CameraSpace::StateType>();
        temp->setT(0.553213);
        temp->rotation().x = 0.606443;
        temp->rotation().y = 0.223499;
        temp->rotation().z = -0.229746;
        temp->rotation().w = 0.727662; //setAxisAngle(0.606443, 0.223499, -0.229746, 0.727662);
        Motion *tmpMotion = new Motion(si_);
        si_->copyState(tmpMotion->state, tmpState);
        Motion *motion = tree_[tree_.size() - 1];
        tmpMotion->parent = motion;
        tree_.push_back(tmpMotion);
        solution = tmpMotion;
        solutionFound_ = true;*/


    while (ptc == false)
    {
    	/* find state to add */
        Motion *smotion = tree_[tree_.size() - 1];
        ompl::base::State *sstate = smotion->state;
        fb::CameraSpace::StateType *cs1=sstate->as<fb::CameraSpace::StateType>();

    	//try the same view angle first to reduce the camera shaking
    	ompl::base::State *nstate = si_->allocState();
    	if (tree_.size() == 1)
    	{
    		sampler_->sample(rstate);
    		si_->copyState(nstate, rstate);
    	}
    	else
    		si_->copyState(nstate, sstate);

    	fb::CameraSpace::StateType *cs=nstate->as<fb::CameraSpace::StateType>();
    	if(cs1->getT() + timeRange_<1)
    	  cs->setT(cs1->getT() + timeRange_);
    	else
    	  cs->setT(1);

    	// Collect the information about nearby obstacles

    	if ((si_->isValid(nstate)) && (si_->checkMotion(sstate, nstate)))
    	{
    		/* create a motion */
    		Motion *nmotion = new Motion(si_);
    	    si_->copyState(nmotion->state, nstate);
    	    nmotion->parent = smotion;
            tree_.push_back(nmotion);
            solved = true;
            si_->printState(nstate, std::cout);
    	}
    	else
    	{
    		/* sample random state */
	        sampler_->sample(rstate);
	        ompl::base::State *dstate = rstate;
	        fb::CameraSpace::StateType* cs2=dstate->as<fb::CameraSpace::StateType>();

	        if(cs1->getT() < cs2->getT())
	        {
	        	double d = cs2->getT() - cs1->getT();

    		    if (d > timeRange_)
    		    {
    		     	//si_->getStateSpace()->interpolate(sstate, rstate, timeRange_ / d, xstate);
    		     	//dstate = xstate;
    		    	cs2->setT(cs1->getT() + timeRange_);
    		    }

    		    if ((si_->isValid(dstate)) && (si_->checkMotion(sstate, dstate)))
    		    {
    		    	//cs2 = dstate->as<fb::CameraSpace::StateType>();
    		    	Motion *nmotion = new Motion(si_);
    		    	si_->copyState(nmotion->state, dstate);
    		    	nmotion->parent = smotion;
    		    	tree_.push_back(nmotion);
    		    	solved = true;
    		    	si_->printState(dstate, std::cout);
    		    }
    		    else
    		    	continue;
	        }
	        else
	        	continue;
    	}

    	Motion *lmotion = tree_[tree_.size() - 1];
        double dist = 0.0;
        bool sat = goal->isSatisfied(lmotion->state, &dist);
        if (sat)
        {
        	solution = lmotion;
        	solutionFound_ = true;
        	break;
        }

        // Check if there is a path from the state to the goal
        fb::CameraSpace::StateType* cs3=lmotion->state->as<fb::CameraSpace::StateType>();

        if ((cs3->getT()<=csg->getT()) && (csg->getT() - cs3->getT() <= timeRange_))
        {
        	if (si_->checkMotion(lmotion->state, sg))
        	{
        		/* create a motion */
        		Motion *gmotion = new Motion(si_);
        		si_->copyState(gmotion->state, sg);
        		gmotion->parent = lmotion;
        		tree_.push_back(gmotion);
        		solution = gmotion;
        		si_->printState(sg, std::cout);
        		solutionFound_ = true;
        	}
        }

        break;
    }

    if (solutionFound_)
    {
    	if (solution != NULL)
    	{
    		lastGoalMotion_ = solution;
    		solved = true;
    	}

    	/* construct the solution path */
    	std::vector<Motion*> mpath;
    	while (solution != NULL)
    	{
    		mpath.push_back(solution);
    		solution = solution->parent;
    	}

    	/* set the solution path */
    	ompl::geometric::PathGeometric *path = new ompl::geometric::PathGeometric(si_);
    	for (int i = mpath.size() - 1 ; i >= 0 ; --i)
    		path->append(mpath[i]->state);
    	pdef_->addSolutionPath(ompl::base::PathPtr(path), false, 0);
    }

    si_->freeState(rstate);
    si_->freeState(xstate);

    return ompl::base::PlannerStatus(solved, false);
}

void flyingcamera::base::moving::CameraPlanner::getPlannerData(ompl::base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    if (lastGoalMotion_)
        data.addGoalVertex(ompl::base::PlannerDataVertex(lastGoalMotion_->state));

    for (unsigned int i = 0 ; i < tree_.size() ; ++i)
    {
        if (tree_[i]->parent == NULL)
            data.addStartVertex(ompl::base::PlannerDataVertex(tree_[i]->state));
        else
            data.addEdge(ompl::base::PlannerDataVertex(tree_[i]->parent->state),
                         ompl::base::PlannerDataVertex(tree_[i]->state));
    }
}

bool flyingcamera::base::moving::CameraPlanner::isSolutionFound() const
{
	return solutionFound_;
}

double flyingcamera::base::moving::CameraPlanner::getTime() const
{
	if (tree_.size() > 0)
	{
		Motion *motion = tree_[tree_.size() - 1];
	    ompl::base::State *state = motion->state;
	    const fb::CameraSpace::StateType *cs=state->as<fb::CameraSpace::StateType>();
	    return cs->getT();
	}

	return 0;
}
